#include <math.h>
#include <stdio.h>
#include <sstream>
#include <iostream>
#include <string>
#include <cstring>

#include <webots/Robot.hpp>
#include <webots/DifferentialWheels.hpp>


#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
// PCL specific includes
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

// %EndTag(MSG_HEADER)%


using namespace std;
using namespace webots;




class RWKinect
{
public:
  RWKinect();
  void init(webots::Robot*,ros::NodeHandle*,string);
  ~RWKinect();

  void iterate();
private:
  double time_step;
  double depcam_width, depcam_height,depcam_min_range,depcam_max_range,depcam_hfoV,depcam_vfoV;
  float phi_min,theta_x_max;
  double rgbcam_width,rgbcam_height;
  Camera* DEPCAM;
  Camera* RGBCAM;
  const float *lms_values;
  ros::NodeHandle* n;
  ros::Publisher depcam_image_pub;
  ros::Publisher rgbcam_image_color_pub;
  ros::Publisher rgbcam_image_mono_pub;
  ros::Publisher depcam_points_pub;
  ros::Publisher rgbcam_points_pub;
  sensor_msgs::Image DepcamImage;
  sensor_msgs::Image RgbcamImageColor;
  sensor_msgs::Image RgbcamImageMono;
  sensor_msgs::PointCloud2 DepcamPoints;
  sensor_msgs::PointCloud2 RgbcamPoints;

  pcl::PointCloud<pcl::PointXYZ> cloud_xyz;
  pcl::PointCloud<pcl::PointXYZRGB> cloud_xyzrgb;
};
